#include<stdio.h>
//#include<Mesh/Mesh_Frame.h>
//#include<Mesh/lib_cell_Iterator.h>
#include<Mesh_IO/Mesh_IO.h>
//#include<Algorithm/Algorithm1.h>
//#include<Algorithm/Algorithm2.h>
#include<tool/libcell_tools_formats.h>
#include<tool/libcell_tools_view.h>
//#include<Algorithm/Harmonic.h>
//#include <Algorithm/Conformal.h>
#include<tool/libcell_tools_octree.h>
#include<tool/libcell_tools_algorithm.h>
#include<Mesh/libcell_iterator.h>
#include <Algorithm/qem.h>
#include<Algorithm/points_cloud_algorithm.h>
#include<libcell_macros.h>

#define _ReadOff_   LibCell_ReadOff_
#define _ReadCell_  LibCell_ReadCell_
#define _ReadPly_ LibCell_ReadPly_
#define _WriteOff_ LibCell_WriteOff_

#define _WriteCell_ LibCell_WriteCell_

static double compute_triangle_energy(template_v* v1,
    template_v* v2,
    template_v* v3)
{
    double a= distance_of_two_points(v2->point,v3->point,3),
        b= distance_of_two_points(v1->point,v3->point,3),
        c= distance_of_two_points(v1->point,v2->point,3);

    double cosa = (b*b+c*c-a*a)/(2*b*c);
    double cosb = (a*a+c*c-b*b)/(2*a*c);


    double anglea = acos(cosa) , angleb = acos(cosb);

    return (anglea>=angleb?anglea:angleb);
}


typedef struct Suture_Vertex{
    // 0 是上 ,1是下
    int shang_or_xia;
    int i;
    template_v* v;
}Suture_Vertex;

static inline void suture_vertex_init(Suture_Vertex*sv)
{
    sv->shang_or_xia=0;
    sv->i=0;
    sv->v=NULL;
}

// is_loop： vb1和vb2 是否构成loop
// 

Node* libcell_tools_suture_two_boundarries(
    Suture_Vertex** vb11,int len1,
    Suture_Vertex** vb22,int len2, 
    int is_loop)
{
    Node* re=NULL;
    int wei1=0,wei2=0;
    // int len1=0,len2=0;
    if(is_loop)
    {
        double temp_dis=distance_of_two_points(
        vb11[wei1]->v->point,vb22[wei2]->v->point,3);
        for(int i=1;i<len2;i++)
        {
            double temp_dis1=distance_of_two_points(
                vb11[wei1]->v->point,vb22[i]->v->point,3);
            if(temp_dis1<temp_dis)
            {
                temp_dis=temp_dis1;
                wei2=i;
            }
        }
    }
    
    // printf("len1:%d len2:%d\n",len1,len2);
    int i=0,j=0;
    int thre_len1=len1,thre_len2=len2;
    if(!is_loop)
    {
        thre_len1--;
        thre_len2--;
    }
    while(i<thre_len1||j<thre_len2)
    {
        Suture_Vertex**vs=(Suture_Vertex**)malloc(
            sizeof(Suture_Vertex*)*3);
        // re=node_overlying(re,vs);
        re=node_pushback(re,vs);
        vs[0]=vb11[((wei1+i)%len1)];
        vs[1]=vb22[((wei2+j)%len2)];
        // printf("wei1:%d wei2:%d\n",wei1,wei2);
        if(i<thre_len1)
        {
            if(j<thre_len2)
            {
                double ener1=compute_triangle_energy(
                    vs[0]->v,vs[1]->v,vb11[((wei1+i+1)%len1)]->v);
                double ener2=compute_triangle_energy(
                    vs[0]->v,vs[1]->v,vb22[((wei2+j+1)%len2)]->v);       
                if(ener1<ener2)
                {
                    vs[2] = vb11[((wei1+i+1)%len1)];
                    // printf("%d %d vb11:%d\n",vs[0]->id,vs[1]->id,vs[2]->id );
                    i++;
                }
                else
                {
                    vs[2] = vb22[((wei2+j+1)%len2)];
                    // printf("%d %d vb22:%d\n",
                    //     vs[0]->id,vs[1]->id,vs[2]->id );
                    j++;
                }
            }
            else
            {
                vs[2] = vb11[((wei1+i+1)%len1)] ;
                // printf("%d %d vb11:%d\n",vs[0]->id,vs[1]->id,vs[2]->id );
                i++;
            }
        }
        else
        {
            vs[2] = vb22[((wei2+j+1)%len2)];
            // printf("%d %d vb22:%d\n",
            //             vs[0]->id,vs[1]->id,vs[2]->id );
            j++;
        }
        // printf("%d %d %d",)
        // printf("i:%d j:%d\n",i,j);
    }

    return node_reverse(re);
}


static void libcell_fill_vb_index_according_sampling_dis(
    Suture_Vertex**vb,int len,double sampling_dis,
    Suture_Vertex** vb_s,int* lens)
{
    *lens=0;
    if(vb==NULL||len<=0){return;}
    int i=0;
    while(i<len)
    {
        // printf("lens:%d\n",*lens);
        vb_s[*lens]=vb[i];
        *lens=1+*lens;
        i++;
        // printf("1i:%d\n",i);
        for(;i<len;i++)
        {
            double dis=distance_of_two_points(
                vb_s[*lens-1]->v->point,
                vb[i]->v->point,3);
            // printf("dis:%lf\n",dis);
            if(dis >= sampling_dis)
            {
                break;
            }  
        }
        // printf("i:%d\n",i);
    }
}

// static Suture_Vertex** libcell_create_array_from_node(Node*n,int *len)
// {
//     *len=node_size(n);
//     Suture_Vertex**vb11=(Suture_Vertex**)malloc(sizeof(Suture_Vertex*)*len1);
//     int i=0;
//     for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
//     {
//         re[i]=nit->value;
//         i++;
//     }
//     return re;
// }



// Node* libcell_tools_suture_two_subset_boundarries(
//     Suture_Vertex** vb11,int len1,
//     Suture_Vertex** vb22,int len2,
//     int i ,int temp_i)
// {

//     for(int k=0;k)
//     {

//     }



// }


// 
//  这个is_loop好像失效了
// 
Node* libcell_my_get_detail_suture_result(
    Suture_Vertex** vb11,int len1,
    Suture_Vertex** vb22,int len2,
    Node*tris)
{
    Node* re=NULL;
    Suture_Vertex**vs=(Suture_Vertex**)(tris->value);
    int i=vs[0]->i,j=vs[1]->i;
    Node*nit=tris;
    int temp_i=i,temp_j=j;
    Suture_Vertex** vbs1=(Suture_Vertex**)malloc(sizeof(Suture_Vertex*)*len1);
    Suture_Vertex** vbs2=(Suture_Vertex**)malloc(sizeof(Suture_Vertex*)*len2);
    while(nit!=NULL)
    {
        temp_i=i;temp_j=j;
        for(;nit!=NULL;nit=(Node*)(nit->Next))
        {
            // Suture_Vertex**vs=(Suture_Vertex**)(nit->value);
            vs=(Suture_Vertex**)(nit->value);
            if(vs[2]->shang_or_xia==0)
            {
                temp_i=vs[2]->i; 
            }
            else
            {
                temp_j=vs[2]->i;
            }
            if(temp_i!=i&&temp_j!=j){break;}
        }
        printf("i:%d j:%d temp_i:%d temp_j:%d\n",i,j,temp_i,temp_j);

        memset(vbs1,0,sizeof(Suture_Vertex*)*len1);
        memset(vbs2,0,sizeof(Suture_Vertex*)*len2);

        int lens1=0,lens2=0;
        
        for(int k=0;k<len1;k++)
        {
            int k1=(k+i)%len1;
            vbs1[lens1]=vb11[k1];
            lens1++;

            if(k1==temp_i){break;}
        }

        for(int k=0;k<len2;k++)
        {
            int k1=(k+j)%len2;
            vbs2[lens2]=vb22[k1];
            lens2++;
            if(k1==temp_j){break;}
        }

        Node*noden= libcell_tools_suture_two_boundarries(
            vbs1, lens1,
            vbs2, lens2,
            0);

        for(Node* nit1=noden;nit1!=NULL;nit1=(Node*)(nit1->Next))
        {
            Suture_Vertex**vs1=(Suture_Vertex**)(nit1->value);
            printf("%d %d %d %d %d %d\n",vs1[0]->i,vs1[1]->i,vs1[2]->i,
                vs1[0]->v->id,vs1[1]->v->id,vs1[2]->v->id);
        }


        

        re=node_plus(re,noden);

        i=temp_i;j=temp_j;
        if(nit==NULL)
        {
            break;
        }
        nit=(Node*)(nit->Next);
    }


    free(vbs1);free(vbs2);
    return re;
}


// 新版本的缝合边界，会对边界进行重采样
// sampling_dis :采样距离
// 
Node* libcell_tools_suture_two_boundarriesn(Node* vb1,
    Node* vb2,int is_loop,double sampling_dis)
{
    int len1=0,len2=0;
    len1=node_size(vb1);
    len2=node_size(vb2);

    Suture_Vertex**vb11=(Suture_Vertex**)malloc(sizeof(Suture_Vertex*)*len1);
    Suture_Vertex**vb22=(Suture_Vertex**)malloc(sizeof(Suture_Vertex*)*len2);

    int i=0;
    for(Node* nit=vb1;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Suture_Vertex*sv=(Suture_Vertex*)malloc(sizeof(Suture_Vertex));
        suture_vertex_init(sv);
        vb11[i]=sv;
        sv->v=(template_v*)(nit->value);
        sv->i=i;
        i++;
    }

    i=0;
    for(Node* nit=vb2;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Suture_Vertex*sv=(Suture_Vertex*)malloc(sizeof(Suture_Vertex));
        suture_vertex_init(sv);
        vb22[i]=sv;
        sv->v=(template_v*)(nit->value);
        sv->i=i;
        sv->shang_or_xia=1;
        i++;
    }

    // Node* re=NULL;

    Suture_Vertex**vbs1=(Suture_Vertex**)malloc(sizeof(Suture_Vertex*)*len1);
    Suture_Vertex**vbs2=(Suture_Vertex**)malloc(sizeof(Suture_Vertex*)*len2);
    int lens1=0,lens2=0;
    // printf("here1 len1:%d len2:%d\n",len1,len2);
    libcell_fill_vb_index_according_sampling_dis(
        vb11,len1,sampling_dis,
        vbs1,&lens1);
    // printf("here2\n");

    libcell_fill_vb_index_according_sampling_dis(
        vb22,len2,sampling_dis,
        vbs2,&lens2);

    printf("begin tris\n");
    Node*tris= libcell_tools_suture_two_boundarries(
        vbs1,lens1,
        vbs2,lens2, 
        is_loop);


    printf("end tris\n");
    Node* re=NULL;

    // re=tris;
    re= libcell_my_get_detail_suture_result(
        vb11,len1,
        vb22,len2,
        tris);

    for(Node* nit=re;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Suture_Vertex**vs=(Suture_Vertex**)(nit->value);

        // printf("%d %d %d\n",vs[0]->v->id,vs[1]->v->id,vs[2]->v->id);
        template_v**vs1=(template_v**)malloc(sizeof(template_v*)*3);
        nit->value=vs1;
        vs1[0]=vs[0]->v;vs1[1]=vs[1]->v;vs1[2]=vs[2]->v;

        free(vs);

    }

    free_node_value(tris);
    free_node(tris);

    free(vbs1);free(vbs2);

    free(vb11);free(vb22);
    return re;
}

int main()
{
    Mesh m1,m2;
    Mesh_init(&m1);Mesh_init(&m2);

    _ReadOff_(&m1 ,"vb1.off" ,3);
    _ReadOff_(&m2 ,"vb2.off" ,3);
    Node* vb1=NULL,*vb2=NULL;
    for(auto vit=m1.v_begin(&m1);vit!=m1.v_end(&m1);vit++)
    {
        // vb1=node_overlying(vb1,quote(vit));
        vb1=node_pushback(vb1,quote(vit));
        // printf("%lf %lf %lf\n",quote(vit)->point[0],
        //     quote(vit)->point[1],
        //     quote(vit)->point[2]);
    }
    printf("vb1 nums:%d %d\n",m1.num_v(&m1),((template_v*)(vb1->value))->id );
    vb1=node_reverse(vb1);
    printf("vb2\n");

    for(auto vit=m2.v_begin(&m2);vit!=m2.v_end(&m2);vit++)
    {
        // vb2=node_overlying(vb2,quote(vit));
        vb2=node_pushback(vb2,quote(vit));
        // printf("%lf %lf %lf\n",quote(vit)->point[0],
        //     quote(vit)->point[1],
        //     quote(vit)->point[2]);
    }
    printf("vb2 nums:%d %d\n",m2.num_v(&m2),((template_v*)(vb2->value))->id );

    vb2=node_reverse(vb2);

    // Node*tris= libcell_tools_suture_two_boundarries(vb1,vb2,1);
    Node* tris= libcell_tools_suture_two_boundarriesn(vb1,
        vb2,1,0.5);
    
    Mesh* m=(Mesh*)malloc(sizeof(Mesh));
    Mesh_init(m);

    m->dimension=2;m->simplex=1;m->manifold_require=1;
    int num=0;
    for(Node* nit=tris;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v**vs=(template_v**)(nit->value);
        printf("%d %d %d\n",vs[0]->id,vs[1]->id,vs[2]->id);
        template_v*vs1[3]={0};
        vs1[0]=m->create_vertexv(m,vs[0]->point,3);
        vs1[1]=m->create_vertexv(m,vs[1]->point,3);
        vs1[2]=m->create_vertexv(m,vs[2]->point,3);
        m->create_cellv(m,vs1,3);
        // if(num>827){break;}
        num++;
    }

    if(m->num_v(m)>0)
    {
        _WriteOff_(m,"ce.off");
    }


    return 0;
}